#include "multiconn_client.h"

#include <zephyr/logging/log.h>
#include <zephyr/kernel.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/sys/mempool.h> // 使用内存池替代 k_malloc/k_free

LOG_MODULE_REGISTER(multiconn_client, CONFIG_MULTICONN_CLIENT_LOG_LEVEL);

// --- 内存池定义 ---
// 根据 Kconfig 定义的大小创建内存池块
#define MAX_SERVICES (CONFIG_BT_MAX_CONN * CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN)
#define MAX_CHARS    (MAX_SERVICES * CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE)
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
#define MAX_DESCS    (MAX_CHARS * CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR)
K_MEM_SLAB_DEFINE(desc_slab, sizeof(multiconn_descriptor_t), MAX_DESCS, __alignof__(multiconn_descriptor_t));
#endif
K_MEM_SLAB_DEFINE(char_slab, sizeof(multiconn_characteristic_t), MAX_CHARS, __alignof__(multiconn_characteristic_t));
K_MEM_SLAB_DEFINE(svc_slab, sizeof(multiconn_service_t), MAX_SERVICES, __alignof__(multiconn_service_t));


// --- 内部状态和变量 ---

static struct multiconn_conn_context conn_contexts[CONFIG_BT_MAX_CONN];
static const multiconn_client_callbacks_t *app_callbacks;
static struct bt_conn_cb conn_callbacks;

// UUID 过滤器
static const struct bt_uuid **filter_svc_uuids;
static size_t filter_svc_count;
static const struct bt_uuid **filter_char_uuids;
static size_t filter_char_count;
static bool filter_enabled = false;


// --- 前向声明 ---
static struct multiconn_conn_context *find_context(struct bt_conn *conn);
static struct multiconn_conn_context *find_free_context(void);
static void cleanup_context_data(struct multiconn_conn_context *ctx);
static void schedule_next_action(struct multiconn_conn_context *ctx);
static void mcc_work_handler(struct k_work *work);
static void mcc_mtu_exchange_cb(struct bt_conn *conn, uint8_t err, struct bt_gatt_exchange_mtu_params *params);
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
static void mcc_le_param_updated(struct bt_conn *conn, uint16_t interval, uint16_t latency, uint16_t timeout);
#endif
static uint8_t mcc_discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, struct bt_gatt_discover_params *params);
static uint8_t mcc_discover_characteristic_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, struct bt_gatt_discover_params *params);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
static uint8_t mcc_discover_descriptor_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr, struct bt_gatt_discover_params *params);
#endif
static bool should_discover_service(const struct bt_uuid *uuid);
static bool should_discover_characteristic(const struct bt_uuid *uuid);


// --- Zephyr 连接回调实现 ---

static void mcc_connected(struct bt_conn *conn, uint8_t err)
{
    char addr[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));

    if (err) {
        LOG_ERR("Connection failed to %s (err %u)", addr, err);
        return;
    }

    LOG_INF("Connected: %s, conn %p", addr, conn);

    struct multiconn_conn_context *ctx = find_free_context();
    if (!ctx) {
        LOG_ERR("No free context for connection %s", addr);
        bt_conn_disconnect(conn, BT_HCI_ERR_CONN_LIMIT_EXCEEDED);
        return;
    }

    // 初始化上下文
    ctx->conn = bt_conn_ref(conn);
    ctx->in_use = true;
    ctx->state = MULTICONN_STATE_CONNECTED;
    ctx->last_err = 0;
    sys_slist_init(&ctx->services);
    ctx->service_count = 0;
    ctx->current_service_node = NULL;
    ctx->current_char_node = NULL;

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
    // 设置期望的连接参数 (从 Kconfig 读取)
    ctx->desired_conn_params.interval_min = CONFIG_MULTICONN_CLIENT_CONN_PARAM_MIN_INT;
    ctx->desired_conn_params.interval_max = CONFIG_MULTICONN_CLIENT_CONN_PARAM_MAX_INT;
    ctx->desired_conn_params.latency = CONFIG_MULTICONN_CLIENT_CONN_PARAM_LATENCY;
    ctx->desired_conn_params.timeout = CONFIG_MULTICONN_CLIENT_CONN_PARAM_TIMEOUT;
#endif

    // 初始化并提交工作项
    k_work_init(&ctx->work, mcc_work_handler);
    schedule_next_action(ctx);
}

static void mcc_disconnected(struct bt_conn *conn, uint8_t reason)
{
    char addr[BT_ADDR_LE_STR_LEN];
    bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
    LOG_INF("Disconnected: %s (reason 0x%02x), conn %p", addr, reason, conn);

    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) {
        LOG_WRN("Disconnected context not found for %s", addr);
        return;
    }

    // 取消可能正在进行的工作
    // k_work_cancel(&ctx->work); // 如果工作队列支持取消

    // 清理资源
    cleanup_context_data(ctx);
    ctx->state = MULTICONN_STATE_DISCONNECTED;
    ctx->in_use = false;
    bt_conn_unref(ctx->conn); // 释放引用
    ctx->conn = NULL;
    ctx->last_err = 0; // 重置错误状态

    // 通知应用层
    if (app_callbacks && app_callbacks->disconnected) {
        app_callbacks->disconnected(ctx, reason);
    }
}

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
// 连接参数更新回调
static void mcc_le_param_updated(struct bt_conn *conn, uint16_t interval,
                                 uint16_t latency, uint16_t timeout)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) {
        LOG_WRN("Param update CB: Context not found for conn %p", conn);
        return;
    }

    // 检查是否处于等待更新的状态
    if (ctx->state != MULTICONN_STATE_CONN_PARAM_UPDATING) {
         LOG_WRN("Param update CB received in unexpected state %d for conn %p", ctx->state, conn);
         // 仍然可以记录参数，但不驱动状态机
         LOG_INF("Params updated (unexpectedly): interval %u, latency %u, timeout %u",
                interval, latency, timeout);
         return;
    }


    LOG_INF("Connection parameters updated for conn %p: interval %u (%d ms), latency %u, timeout %u (%d ms)",
            conn, interval, (int)(interval * 1.25), latency, timeout, timeout * 10);

    // 进入下一个状态：服务发现
    ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
    schedule_next_action(ctx);
    // 注意：Zephyr 的 bt_conn_le_param_update() 成功调用不保证外设接受更新。
    // 这个回调表示更新过程已完成（无论接受或拒绝，参数可能没变）。
    // 如果需要区分接受/拒绝，可能需要更复杂的逻辑或检查更新后的参数是否符合预期。
}
#endif

// --- GATT 回调实现 ---

static void mcc_mtu_exchange_cb(struct bt_conn *conn, uint8_t err,
                                struct bt_gatt_exchange_mtu_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) {
        LOG_ERR("MTU CB: Context not found for conn %p", conn);
        return;
    }

    if (ctx->state != MULTICONN_STATE_MTU_EXCHANGING) {
         LOG_WRN("MTU CB received in unexpected state %d for conn %p", ctx->state, conn);
         return; // 不要意外地驱动状态机
    }


    if (err) {
        LOG_ERR("MTU exchange failed for conn %p (err %u)", conn, err);
        ctx->last_err = err; // ATT 错误码
        ctx->state = MULTICONN_STATE_ERROR;
    } else {
        uint16_t mtu = bt_gatt_get_mtu(conn);
        LOG_INF("MTU updated to %u for conn %p", mtu, conn);
        // 成功，进入下一个状态
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
        ctx->state = MULTICONN_STATE_CONN_PARAM_UPDATING;
#else
        ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
#endif
    }

    schedule_next_action(ctx);
}

static uint8_t mcc_discover_service_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                       struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) { return BT_GATT_ITER_STOP; }

     if (ctx->state != MULTICONN_STATE_SERVICE_DISCOVERING) {
         LOG_WRN("Service Discover CB received in unexpected state %d for conn %p", ctx->state, conn);
         return BT_GATT_ITER_STOP; // 停止，防止意外行为
     }


    if (!attr) { // 发现完成
        LOG_INF("Service discovery complete for conn %p (found %u services)", conn, ctx->service_count);
        if (ctx->last_err == 0) {
            if (ctx->service_count > 0) {
                ctx->state = MULTICONN_STATE_CHAR_DISCOVERING;
                ctx->current_service_node = sys_slist_peek_head(&ctx->services); // 从第一个服务开始
            } else {
                LOG_INF("No matching services found for conn %p.", conn);
                ctx->state = MULTICONN_STATE_READY; // 没有服务也认为准备就绪
            }
        } else { // Discovery stopped due to an error earlier (e.g., mem limit)
            ctx->state = MULTICONN_STATE_ERROR;
        }
        schedule_next_action(ctx);
        return BT_GATT_ITER_STOP;
    }

    if (params->type == BT_GATT_DISCOVER_PRIMARY) {
        struct bt_gatt_service_val *svc_val = (struct bt_gatt_service_val *)attr->user_data;
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(svc_val->uuid, uuid_str, sizeof(uuid_str));

        // 应用过滤器
        if (!should_discover_service(svc_val->uuid)) {
            LOG_DBG("Skipping filtered service UUID %s", uuid_str);
            return BT_GATT_ITER_CONTINUE;
        }

        LOG_DBG("Found service: UUID=%s, start_handle=0x%04x, end_handle=0x%04x",
                uuid_str, attr->handle, svc_val->end_handle);

        // 检查内存池和限制
        if (ctx->service_count >= CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN) {
             LOG_WRN("Max services per connection reached (%d)", CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN);
             ctx->last_err = -ENOBUFS; // Indicate limit reached
             return BT_GATT_ITER_CONTINUE; // Continue searching but don't store more
        }


        // 分配并存储服务信息 (使用内存池)
        multiconn_service_t *new_svc;
        if (k_mem_slab_alloc(&svc_slab, (void **)&new_svc, K_NO_WAIT) != 0) {
             LOG_ERR("Failed to allocate memory for service from slab");
             ctx->last_err = -ENOMEM;
             return BT_GATT_ITER_STOP; // Stop discovery
        }
        memset(new_svc, 0, sizeof(multiconn_service_t)); // 清零

        // 复制 UUID
        bt_uuid_create(&new_svc->uuid, svc_val->uuid->data, bt_uuid_get_len(svc_val->uuid));
        new_svc->start_handle = attr->handle;
        new_svc->end_handle = svc_val->end_handle;
        sys_slist_init(&new_svc->characteristics);
        new_svc->char_count = 0;

        sys_slist_append(&ctx->services, &new_svc->node);
        ctx->service_count++;
    }

    return BT_GATT_ITER_CONTINUE;
}

static uint8_t mcc_discover_characteristic_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                              struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) { return BT_GATT_ITER_STOP; }
    if (!ctx->current_service_node) { LOG_ERR("Char CB: No current service context"); return BT_GATT_ITER_STOP; }
    struct multiconn_service_t *current_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);

     if (ctx->state != MULTICONN_STATE_CHAR_DISCOVERING) {
         LOG_WRN("Char Discover CB received in unexpected state %d for conn %p", ctx->state, conn);
         return BT_GATT_ITER_STOP;
     }


    if (!attr) { // 当前服务的特征发现完成
        char svc_uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(¤t_svc->uuid, svc_uuid_str, sizeof(svc_uuid_str));
        LOG_INF("Characteristic discovery complete for service UUID %s on conn %p (found %u chars)",
                 svc_uuid_str, conn, current_svc->char_count);

        if (ctx->last_err != 0) {
            ctx->state = MULTICONN_STATE_ERROR;
            schedule_next_action(ctx);
            return BT_GATT_ITER_STOP;
        }

        // 移至下一个服务
        ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
        if (ctx->current_service_node != NULL) {
            LOG_DBG("Moving to next service for characteristic discovery.");
            // 状态保持 CHAR_DISCOVERING
        } else {
            // 所有服务的特征都发现完毕
            LOG_INF("All characteristics discovered for conn %p.", conn);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
             // 如果有服务且需要发现描述符，则进入描述符发现阶段
             if (ctx->service_count > 0) {
                 ctx->state = MULTICONN_STATE_DESC_DISCOVERING;
                 // 重置迭代器到第一个服务和它的第一个特征（如果存在）
                 ctx->current_service_node = sys_slist_peek_head(&ctx->services);
                 current_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                 ctx->current_char_node = sys_slist_peek_head(¤t_svc->characteristics);
             } else {
                 ctx->state = MULTICONN_STATE_READY; // 没有服务，直接就绪
             }
#else
            ctx->state = MULTICONN_STATE_READY; // 不发现描述符，直接就绪
#endif
        }
        schedule_next_action(ctx);
        return BT_GATT_ITER_STOP;
    }

    if (params->type == BT_GATT_DISCOVER_CHARACTERISTIC) {
        struct bt_gatt_chrc *chrc_val = (struct bt_gatt_chrc *)attr->user_data;
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(chrc_val->uuid, uuid_str, sizeof(uuid_str));

        // 应用过滤器
        if (!should_discover_characteristic(chrc_val->uuid)) {
             LOG_DBG("Skipping filtered characteristic UUID %s", uuid_str);
             return BT_GATT_ITER_CONTINUE;
        }

        LOG_DBG("  Found characteristic: UUID=%s, decl_handle=0x%04x, value_handle=0x%04x, props=0x%02x",
                uuid_str, attr->handle, chrc_val->value_handle, chrc_val->properties);

        // 检查内存池和限制
         if (current_svc->char_count >= CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE) {
             LOG_WRN("Max characteristics per service reached (%d)", CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE);
             ctx->last_err = -ENOBUFS;
             return BT_GATT_ITER_CONTINUE; // Continue but don't store
         }


        // 分配并存储特征信息
        multiconn_characteristic_t *new_char;
         if (k_mem_slab_alloc(&char_slab, (void **)&new_char, K_NO_WAIT) != 0) {
            LOG_ERR("Failed to allocate memory for characteristic from slab");
            ctx->last_err = -ENOMEM;
            return BT_GATT_ITER_STOP;
        }
         memset(new_char, 0, sizeof(multiconn_characteristic_t)); // 清零

        // 复制 UUID
        bt_uuid_create(&new_char->uuid, chrc_val->uuid->data, bt_uuid_get_len(chrc_val->uuid));
        new_char->decl_handle = attr->handle;
        new_char->value_handle = chrc_val->value_handle;
        new_char->properties = chrc_val->properties;
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
        sys_slist_init(&new_char->descriptors);
        new_char->desc_count = 0;
        new_char->ccc_handle = 0;
#endif

        sys_slist_append(¤t_svc->characteristics, &new_char->node);
        current_svc->char_count++;
    }

    return BT_GATT_ITER_CONTINUE;
}


#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
static uint8_t mcc_discover_descriptor_cb(struct bt_conn *conn, const struct bt_gatt_attr *attr,
                                          struct bt_gatt_discover_params *params)
{
    struct multiconn_conn_context *ctx = find_context(conn);
    if (!ctx) { return BT_GATT_ITER_STOP; }
    if (!ctx->current_service_node || !ctx->current_char_node) {
        LOG_ERR("Desc CB: Missing current service/char node");
        return BT_GATT_ITER_STOP;
    }
     struct multiconn_characteristic_t *current_char = CONTAINER_OF(ctx->current_char_node, multiconn_characteristic_t, node);

     if (ctx->state != MULTICONN_STATE_DESC_DISCOVERING) {
          LOG_WRN("Descriptor Discover CB received in unexpected state %d for conn %p", ctx->state, conn);
          return BT_GATT_ITER_STOP;
     }


    if (!attr) { // 当前特征的描述符发现完成
        char char_uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(¤t_char->uuid, char_uuid_str, sizeof(char_uuid_str));
        LOG_DBG("Descriptor discovery complete for char UUID %s (found %u descs)",
                char_uuid_str, current_char->desc_count);

         if (ctx->last_err != 0) { // Check if stopped due to previous error
             ctx->state = MULTICONN_STATE_ERROR;
             schedule_next_action(ctx);
             return BT_GATT_ITER_STOP;
         }

        // 移动到下一个特征或服务，并重新调度工作
        // （移动逻辑现在在 work_handler 中处理，这里只需停止当前 discover）
        schedule_next_action(ctx); // Will trigger work handler to find next char/service
        return BT_GATT_ITER_STOP; // Stop discovery for this characteristic
    }

    if (params->type == BT_GATT_DISCOVER_DESCRIPTOR) {
        const struct bt_uuid *desc_uuid = ((struct bt_gatt_attr *)attr)->uuid; // UUID is part of attr
        char uuid_str[BT_UUID_STR_LEN];
        bt_uuid_to_str(desc_uuid, uuid_str, sizeof(uuid_str));
        LOG_DBG("    Found descriptor: UUID=%s, handle=0x%04x", uuid_str, attr->handle);

        // 检查内存池和限制
        if (current_char->desc_count >= CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR) {
            LOG_WRN("Max descriptors per characteristic reached (%d)", CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR);
            ctx->last_err = -ENOBUFS;
            return BT_GATT_ITER_CONTINUE; // Continue finding but don't store
        }

        // 分配并存储描述符信息
         multiconn_descriptor_t *new_desc;
        if (k_mem_slab_alloc(&desc_slab, (void **)&new_desc, K_NO_WAIT) != 0) {
            LOG_ERR("Failed to allocate memory for descriptor from slab");
            ctx->last_err = -ENOMEM;
            return BT_GATT_ITER_STOP; // Stop discovery
        }
        memset(new_desc, 0, sizeof(multiconn_descriptor_t));

        // 复制 UUID 和 handle
        bt_uuid_create(&new_desc->uuid, desc_uuid->data, bt_uuid_get_len(desc_uuid));
        new_desc->handle = attr->handle;

        sys_slist_append(¤t_char->descriptors, &new_desc->node);
        current_char->desc_count++;

        // 检查是否是 CCCD 并存储快捷方式
        if (bt_uuid_cmp(&new_desc->uuid, BT_UUID_GATT_CCC) == 0) {
            current_char->ccc_handle = new_desc->handle;
            LOG_DBG("    -> CCCD Handle stored: 0x%04x", new_desc->handle);
        }
    }

    return BT_GATT_ITER_CONTINUE; // 继续查找下一个描述符
}
#endif // CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE


// --- 工作队列处理函数 ---

static void mcc_work_handler(struct k_work *work)
{
    struct multiconn_conn_context *ctx = CONTAINER_OF(work, struct multiconn_conn_context, work);
    int err = 0;

    LOG_DBG("Work handler: conn %p, state: %d", ctx->conn, ctx->state);

    // 如果连接已断开，则停止处理
    if (!ctx->conn || !ctx->in_use || ctx->state == MULTICONN_STATE_DISCONNECTED) {
        LOG_WRN("Work handler called for inactive/disconnected context %p", ctx);
        return;
    }


    switch (ctx->state) {
        case MULTICONN_STATE_CONNECTED:
            ctx->state = MULTICONN_STATE_MTU_EXCHANGING;
            ctx->mtu_params.func = mcc_mtu_exchange_cb;
            err = bt_gatt_exchange_mtu(ctx->conn, &ctx->mtu_params);
            if (err) {
                LOG_ERR("Failed to start MTU exchange for conn %p (err %d)", ctx->conn, err);
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx); // Re-schedule to handle error state
            } else {
                 LOG_DBG("MTU exchange started for conn %p.", ctx->conn);
            }
            break;

#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
        case MULTICONN_STATE_CONN_PARAM_UPDATING:
            err = bt_conn_le_param_update(ctx->conn, &ctx->desired_conn_params);
            if (err) {
                 // EALREADY 通常是可接受的错误，表示请求已挂起
                 if (err == -EALREADY) {
                     LOG_WRN("Connection parameter update already pending for conn %p.", ctx->conn);
                     // 仍然需要等待 `mcc_le_param_updated` 回调，所以这里不改变状态或调度
                 } else {
                    LOG_ERR("Failed to request conn param update for conn %p (err %d)", ctx->conn, err);
                    // 决定是否算作致命错误。通常可以继续，记录警告。
                    // ctx->last_err = err;
                    // ctx->state = MULTICONN_STATE_ERROR;
                    LOG_WRN("Proceeding despite connection parameter update request failure.");
                    // 跳过等待，直接进入服务发现
                    ctx->state = MULTICONN_STATE_SERVICE_DISCOVERING;
                    schedule_next_action(ctx);
                 }
            } else {
                LOG_DBG("Connection parameter update requested for conn %p.", ctx->conn);
                // 等待 mcc_le_param_updated 回调驱动状态机
            }
            break;
#endif

        case MULTICONN_STATE_SERVICE_DISCOVERING:
            ctx->discover_params.uuid = NULL; // Find all primary
            ctx->discover_params.start_handle = BT_ATT_FIRST_ATTRIBUTE_HANDLE;
            ctx->discover_params.end_handle = BT_ATT_LAST_ATTRIBUTE_HANDLE;
            ctx->discover_params.type = BT_GATT_DISCOVER_PRIMARY;
            ctx->discover_params.func = mcc_discover_service_cb;
            err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
            if (err) {
                LOG_ERR("Failed to start service discovery for conn %p (err %d)", ctx->conn, err);
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
            } else {
                 LOG_DBG("Service discovery started for conn %p.", ctx->conn);
            }
            break;

        case MULTICONN_STATE_CHAR_DISCOVERING:
            if (ctx->current_service_node == NULL) { // 应该在回调中设置好了
                LOG_ERR("Char discovery state but no current service node for conn %p!", ctx->conn);
                ctx->last_err = -EINVAL;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
                break;
            }
             struct multiconn_service_t *svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
            ctx->discover_params.uuid = NULL; // Find all characteristics
            // Handle 范围: 从服务声明句柄+1 到 服务结束句柄
            ctx->discover_params.start_handle = svc->start_handle + 1;
             // 确保 start_handle 不会超过 end_handle (虽然不太可能发生)
             if (ctx->discover_params.start_handle > svc->end_handle) {
                 LOG_WRN("Service start_handle+1 (0x%04x) > end_handle (0x%04x) for conn %p, skipping char discovery for this service.",
                         ctx->discover_params.start_handle, svc->end_handle, ctx->conn);
                 // 手动移动到下一个服务并重新调度
                 ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
                 if (ctx->current_service_node == NULL) { // 没有更多服务了
                     LOG_INF("Finished char discovery stage for conn %p.", ctx->conn);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
                     if (ctx->service_count > 0) { // 如果之前发现了服务
                          ctx->state = MULTICONN_STATE_DESC_DISCOVERING;
                          ctx->current_service_node = sys_slist_peek_head(&ctx->services); // Reset for desc discovery
                          struct multiconn_service_t *first_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                          ctx->current_char_node = sys_slist_peek_head(&first_svc->characteristics);
                     } else {
                          ctx->state = MULTICONN_STATE_READY;
                     }
#else
                     ctx->state = MULTICONN_STATE_READY;
#endif
                 } // else: 状态不变，重新调度以处理下一个服务
                 schedule_next_action(ctx);
                 break; // 退出当前 case
             }

            ctx->discover_params.end_handle = svc->end_handle;
            ctx->discover_params.type = BT_GATT_DISCOVER_CHARACTERISTIC;
            ctx->discover_params.func = mcc_discover_characteristic_cb;

            err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
             if (err) {
                LOG_ERR("Failed to start char discovery for service (start 0x%04x) on conn %p (err %d)",
                        ctx->discover_params.start_handle, ctx->conn, err);
                ctx->last_err = err;
                ctx->state = MULTICONN_STATE_ERROR;
                schedule_next_action(ctx);
            } else {
                char svc_uuid_str[BT_UUID_STR_LEN];
                bt_uuid_to_str(&svc->uuid, svc_uuid_str, sizeof(svc_uuid_str));
                 LOG_DBG("Characteristic discovery started for service UUID %s (handles 0x%04x-0x%04x) on conn %p.",
                         svc_uuid_str, ctx->discover_params.start_handle, ctx->discover_params.end_handle, ctx->conn);
            }
            break;

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
        case MULTICONN_STATE_DESC_DISCOVERING:
            // 找到下一个需要发现描述符的特征
            while (ctx->current_service_node != NULL) {
                struct multiconn_service_t *current_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                // 找到当前服务的下一个特征
                 while (ctx->current_char_node != NULL) {
                     struct multiconn_characteristic_t *current_char = CONTAINER_OF(ctx->current_char_node, multiconn_characteristic_t, node);

                     // 检查是否有空间查找描述符 (value_handle < service_end_handle)
                     if (current_char->value_handle < current_svc->end_handle) {
                          // 开始查找当前特征的描述符
                          ctx->discover_params.uuid = NULL; // Find all descriptors
                          ctx->discover_params.start_handle = current_char->value_handle + 1;
                          ctx->discover_params.end_handle = current_svc->end_handle;
                          ctx->discover_params.type = BT_GATT_DISCOVER_DESCRIPTOR;
                          ctx->discover_params.func = mcc_discover_descriptor_cb;

                          err = bt_gatt_discover(ctx->conn, &ctx->discover_params);
                          if (err) {
                              LOG_ERR("Failed to start desc discovery for char (value 0x%04x) on conn %p (err %d)",
                                      current_char->value_handle, ctx->conn, err);
                              ctx->last_err = err;
                              ctx->state = MULTICONN_STATE_ERROR;
                              schedule_next_action(ctx); // Schedule error handling
                              return; // 退出 work handler
                          } else {
                              char char_uuid_str[BT_UUID_STR_LEN];
                              bt_uuid_to_str(¤t_char->uuid, char_uuid_str, sizeof(char_uuid_str));
                              LOG_DBG("Descriptor discovery started for char UUID %s (handles 0x%04x-0x%04x) on conn %p.",
                                     char_uuid_str, ctx->discover_params.start_handle, ctx->discover_params.end_handle, ctx->conn);
                              return; // 成功启动，等待回调，退出 work handler
                          }
                     } else {
                          // 没有空间查找描述符，跳过此特征
                          LOG_DBG("Skipping desc discovery for char (value 0x%04x): no handles available after value.",
                                  current_char->value_handle);
                          ctx->current_char_node = sys_slist_peek_next(ctx->current_char_node); // 移到下一个特征
                          continue; // 继续内层 while 循环
                     }
                 } // end char loop

                 // 当前服务的所有特征处理完毕，移动到下一个服务
                 ctx->current_service_node = sys_slist_peek_next(ctx->current_service_node);
                 if (ctx->current_service_node) {
                     // 重置特征迭代器到下一个服务的头部
                     struct multiconn_service_t *next_svc = CONTAINER_OF(ctx->current_service_node, multiconn_service_t, node);
                     ctx->current_char_node = sys_slist_peek_head(&next_svc->characteristics);
                 }
             } // end service loop

            // 所有服务及其特征的描述符发现都已尝试或完成
            LOG_INF("All descriptor discovery attempts finished for conn %p.", ctx->conn);
            ctx->state = MULTICONN_STATE_READY;
            schedule_next_action(ctx); // 调度以进入 READY 状态处理
            break;
#endif // CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE

        case MULTICONN_STATE_READY:
            LOG_INF("Connection %p initialization complete, now READY.", ctx->conn);
            if (app_callbacks && app_callbacks->ready) {
                app_callbacks->ready(ctx, 0); // Success
            }
            // 流程结束，不再主动调度
            break;

        case MULTICONN_STATE_ERROR:
            LOG_ERR("Connection %p entered ERROR state (last_err %d). Initialization failed.",
                    ctx->conn, ctx->last_err);
            if (app_callbacks && app_callbacks->ready) {
                // 使用 ready 回调通知失败
                app_callbacks->ready(ctx, ctx->last_err);
            }
            // 停留在错误状态，可以考虑断开连接
            // bt_conn_disconnect(ctx->conn, BT_HCI_ERR_REMOTE_USER_TERM_CONN);
            break;

        default:
            LOG_WRN("Unhandled state %d in work handler for conn %p", ctx->state, ctx->conn);
            break;
    }
}


// --- 内部帮助函数 ---

static struct multiconn_conn_context *find_context(struct bt_conn *conn) { /* ... 不变 ... */ }
static struct multiconn_conn_context *find_free_context(void) { /* ... 不变 ... */ }

// 清理上下文中的动态分配数据（服务、特征、描述符）
static void cleanup_context_data(struct multiconn_conn_context *ctx)
{
     if (!ctx) return;

    sys_snode_t *svc_node, *char_node;
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
    sys_snode_t *desc_node;
#endif

    while ((svc_node = sys_slist_get(&ctx->services))) {
        multiconn_service_t *svc = CONTAINER_OF(svc_node, multiconn_service_t, node);
        while ((char_node = sys_slist_get(&svc->characteristics))) {
             multiconn_characteristic_t *chara = CONTAINER_OF(char_node, multiconn_characteristic_t, node);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
             while((desc_node = sys_slist_get(&chara->descriptors))) {
                  multiconn_descriptor_t *desc = CONTAINER_OF(desc_node, multiconn_descriptor_t, node);
                  k_mem_slab_free(&desc_slab, (void *)desc); // 从 slab 释放描述符
             }
#endif
             k_mem_slab_free(&char_slab, (void *)chara); // 从 slab 释放特征
        }
        k_mem_slab_free(&svc_slab, (void *)svc); // 从 slab 释放服务
    }
    ctx->service_count = 0;
    sys_slist_init(&ctx->services);
}

// 提交工作项到系统工作队列
static void schedule_next_action(struct multiconn_conn_context *ctx)
{
     if (!ctx->conn || !ctx->in_use || ctx->state == MULTICONN_STATE_DISCONNECTED) {
         LOG_WRN("Attempted to schedule action for inactive/disconnected context %p", ctx);
         return;
     }
     k_work_submit(&ctx->work);
}

// 检查是否需要发现此服务 UUID
static bool should_discover_service(const struct bt_uuid *uuid)
{
    if (!filter_enabled || filter_svc_count == 0) {
        return true; // 没有启用过滤或服务过滤器为空，则发现所有
    }
    for (size_t i = 0; i < filter_svc_count; i++) {
        if (bt_uuid_cmp(uuid, filter_svc_uuids[i]) == 0) {
            return true; // 在列表中找到
        }
    }
    return false; // 不在列表中
}

// 检查是否需要发现此特征 UUID
static bool should_discover_characteristic(const struct bt_uuid *uuid)
{
    if (!filter_enabled || filter_char_count == 0) {
        return true; // 没有启用过滤或特征过滤器为空，则发现所有
    }
     for (size_t i = 0; i < filter_char_count; i++) {
        if (bt_uuid_cmp(uuid, filter_char_uuids[i]) == 0) {
            return true; // 在列表中找到
        }
    }
    return false; // 不在列表中
}


// --- 公开函数实现 ---

int multiconn_client_init(const multiconn_client_callbacks_t *callbacks)
{
    if (!callbacks) { return -EINVAL; }
    app_callbacks = callbacks;

    memset(conn_contexts, 0, sizeof(conn_contexts));
    for(int i=0; i<CONFIG_BT_MAX_CONN; ++i) {
        conn_contexts[i].in_use = false;
        // work 在 connected 时初始化
    }

    // 填充并注册 Zephyr 连接回调
    conn_callbacks.connected = mcc_connected;
    conn_callbacks.disconnected = mcc_disconnected;
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
    conn_callbacks.le_param_updated = mcc_le_param_updated;
#endif
    // conn_callbacks.security_changed = ... ; // 如果需要

    bt_conn_cb_register(&conn_callbacks);

    LOG_INF("Multi-connection client library initialized.");
    // Log Kconfig options being used
    LOG_INF("Settings: Max Services/Conn=%d, Max Chars/Service=%d",
             CONFIG_MULTICONN_CLIENT_MAX_SERVICES_PER_CONN,
             CONFIG_MULTICONN_CLIENT_MAX_CHARS_PER_SERVICE);
#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
    LOG_INF("Settings: Max Descs/Char=%d, Descriptor Discovery ENABLED",
            CONFIG_MULTICONN_CLIENT_MAX_DESCS_PER_CHAR);
#else
    LOG_INF("Settings: Descriptor Discovery DISABLED");
#endif
#ifdef CONFIG_MULTICONN_CLIENT_CONN_PARAM_UPDATE_ENABLE
     LOG_INF("Settings: Conn Param Update ENABLED (Min: %d, Max: %d, Latency: %d, Timeout: %d)",
            CONFIG_MULTICONN_CLIENT_CONN_PARAM_MIN_INT, CONFIG_MULTICONN_CLIENT_CONN_PARAM_MAX_INT,
            CONFIG_MULTICONN_CLIENT_CONN_PARAM_LATENCY, CONFIG_MULTICONN_CLIENT_CONN_PARAM_TIMEOUT);
#else
     LOG_INF("Settings: Conn Param Update DISABLED");
#endif
    return 0;
}

int multiconn_client_set_filter(const struct bt_uuid *svc_uuids[], size_t svc_count,
                                const struct bt_uuid *char_uuids[], size_t char_count)
{
    // 简单的赋值，假设传入的指针数组在库生命周期内有效
    // 注意：如果需要更健壮的实现，应该复制这些 UUID 列表
    filter_svc_uuids = svc_uuids;
    filter_svc_count = svc_count;
    filter_char_uuids = char_uuids;
    filter_char_count = char_count;
    filter_enabled = (svc_count > 0 || char_count > 0);

    LOG_INF("UUID Filter %s.", filter_enabled ? "ENABLED" : "DISABLED");
    if (filter_enabled) {
         LOG_INF("Filtering for %zu services and %zu characteristics.", svc_count, char_count);
    }

    return 0;
}

struct multiconn_conn_context *multiconn_client_get_context(struct bt_conn *conn)
{
    return find_context(conn);
}

// 辅助函数实现
multiconn_characteristic_t* multiconn_client_find_characteristic(multiconn_service_t *svc, const struct bt_uuid *uuid)
{
    if (!svc || !uuid) return NULL;
    sys_snode_t *char_node;
    SYS_SLIST_FOR_EACH_NODE(&svc->characteristics, char_node) {
        multiconn_characteristic_t *chara = CONTAINER_OF(char_node, multiconn_characteristic_t, node);
        if (bt_uuid_cmp(&chara->uuid, uuid) == 0) {
            return chara;
        }
    }
    return NULL;
}

#ifdef CONFIG_MULTICONN_CLIENT_DISCOVER_DESCRIPTORS_ENABLE
multiconn_descriptor_t* multiconn_client_find_descriptor(multiconn_characteristic_t *chara, const struct bt_uuid *uuid)
{
     if (!chara || !uuid) return NULL;
     sys_snode_t *desc_node;
     SYS_SLIST_FOR_EACH_NODE(&chara->descriptors, desc_node) {
        multiconn_descriptor_t *desc = CONTAINER_OF(desc_node, multiconn_descriptor_t, node);
        if (bt_uuid_cmp(&desc->uuid, uuid) == 0) {
            return desc;
        }
     }
     return NULL;
}
#endif